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Abstract 


Many  projects  within  the  GRASP  laboratory  involve  motion  control  via  electric 
servo  motors,  for  example  robots,  hands,  camera  mounts  and  tables.  To  date 
each  project  has  been  based  on  a  unique  haadware/software  approach. 

This  document  discusses  the  development  of  a  new  modular,  and  host  inde¬ 
pendent,  nsotor  control  system,  MMCS,  for  laboratory  use.  The  background  to 
the  project  and  the  development  of  the  concept  is  traced. 

An  important  hardware  component  devloped  is  a  2  axis  control  motor  control 
board  that  can  be  plugged  into  an  IBM  PC  bus  or  connected  via  an  adaptor  to 
a  high  performance  workstation  computer. 

To  eliminate  the  need  for  detailed  understanding  of  the  hardware  compo¬ 
nents,  an  abstract  controller  model  is  proposed.  Software  implementing  this 
model  has  been  developed  in  a  device  driver  for  the  Unix  operating  system. 
However  for  those  who  need  or  wish  to  program  at  the  hardware  level,  the  man¬ 
ual  describes  in  detail  the  various  custom  hardware  components  of  the  system. 
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Chapter  1 

Introduction 


1.1  Overview 

This  first  chapter  discusses  the  motivation  for  a  new  modular,  and  host  indepen¬ 
dent,  motor  control  system  for  laboratory  use.  The  background  to  the  project 
and  the  development  of  the  concept  is  traced.  A  number  of  possible  solutions 
are  proposed  and  discussed,  leading  to  a  general  description  of  the  system  that 
has  been  implemented. 

Chapter  2  describes  in  detail  an  abstract  programmer’s  model  of  the  axis 
controller.  Details  of  the  servo  interface  hardware  are  hidden,  allowing  the 
applications  programmer  to  concentrate  on  higher  level  control.  The  software 
implements  position,  velocity  or  torque  control,  selectable  per  axis,  and  the 
closed  loop  dynamics  may  be  modified  by  a  digital  compensation  network. 

Chapter  3  describes  an  interactive  graphical  tool  that  allows  a  user  to  con¬ 
figure  the  axis  controller,  perform  diagnostics  and  perform  joint  level  motions. 

The  last  two  chapters  are  not  essential  reading  for  casual  programmers,  but 
are  essential  for  those  programming  at  the  hardware  level. 

Chapter  4  describes  the  function  performed  by  the  host  bus  adaptor  and 
also  the  axis  controller  bus,  which  is  the  same  as  IBM/PC  bus.  Details  such 
as  redefinition  of  some  signal  lines^,  and  the  addressing  conventions  used  are 
covered.  It  describes  in  detail  the  hardware  implementation  of  the  VME  host 
to  PC  bus  adaptor  that  was  built. 

Chapter  5  describes  in  detail  the  hardware  and  programming  details  for  the 
Mark  I  servo  interface  board. 

Mt’s  not  an  had  as  it  sounds 
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1.2  Background  and  Motivation 

Many  projects  within  the  GRASP  lab.  involve  motion  control  via  electric  servo 
motors,  for  example  robots,  hands,  camera  mounts  and  tables.  Eau:h  project 
has  been  based  on  a  unique  hardware/softwarc  approach.  In  the  last  few  years 
the  approaches  have  included 

•  VAL-II  control  language  receiving  commands  over  a  serial  line  from  a  host 
computer 

•  RCCL  (Hayward  and  Paul) 

•  RFMS  multiprocessor  (Zhang  and  Paul) 

The  first  approach  is  limited  by  communications  speed,  and  is  not  suitable 
for  real-time  sensor  based  control  T*-?  RFMS  controller  has  proved  in  practice 
to  be  very  difficult  to  program,  and  does  not  seem  to  have  realized  the  full 
potential  of  its  parallel  hardware  architecture. 

RCCL  is  a  very  general  robot  programming  environment  and  is  capable  of 
real-time  sensor  based  control,  as  has  been  demonstrated  by  various  projects 
within  the  lab.  It  does  have  the  drawback  that  it  is  tightly  coupled  to  the  VAX 
architecture  and  Unimate  robots  and  their  controllers 

RCCL  provides  the  programmer  with  a  particular  model  of  the  robot  and 
its  environment.  This  model,  based  on  kinematic  position  equations  and  carte¬ 
sian  representation  using  homogeneous  transforms,  is  very  powerful,  however 
there  are  many  applications  to  which  it  is  not  well  suited.  It  is  at  this  point 
that  the  inherent  inflexibility  of  RCCL  becomes  a  problem,  and  the  applica¬ 
tion  programmer’s  effort  goes  increasingly  into  outwitting  and  thwarting  RCCL 
“features”. 

Based  on  discussions  with  robot  users  in  the  laboratory  the  following  points 
were  made 

1.  Robot  control  hardware.  It  was  considered  that  the  best  platform  for  robot 
control  would  be  a  powerful  single  processor  system  like  a  workstation.  A 
single  thread  machine  is  inherently  easier  to  program,  and  a  workstation 
provides  an  integrated  environment  for  program  development  and  high 
speed  execution.  To  allow  a  workstacion  to  perform  robot  control  an 
interface  is  required  to  the  robot’s  electronic  subsystems. 

2.  Robot  interface.  The  RCCL  controllers  use  a  relatively  high  level  inter¬ 
face  to  the  Puma  robot.  The  Unimate  controller  boxes  provide  position 
servo  capability,  A/D^  and  D/A^  converters  etc.  A  functionally  more  gen¬ 
eral  interface  was  designed  for  the  RFMS  project,  but  the  interface  was 
physically  limited  to  use  within  the  RFMS(board  size,  connectors  etc). 

^Analog  to  digital 

^  Distal  to  analog 
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Professor  Paul  commlsioned  a  final  year  project  to  build  a  general  pur¬ 
pose  6  axis  interface  for  the  MicroVAX  Qbus,  but  this  was  never  finished 
and  there  is  some  doubt  as  to  whether  MicroVAXs  and  Qbus  are  the 
hardware  platform  to  use  in  the  future. 

The  author  suggested  a  r^ore  general  solution,  based  cn  the  technology 
developed  for  the  RFMS.  The  axis  controller  would  be  modular,  thus  al¬ 
lowing  it  to  be  expanded  easily  to  cope  with  changing  requirements,  for 
example  7  axis  robot,  robot  -t-  hand,  or  two  cooperating  robots.  Most 
importantly  the  axis  controllers  would  be  independent  of  the  host  proces¬ 
sor  bus,  whether  it  b^  Multibus,  VMEbus  or  Qbus.  A  simple  electronic 
adaptor  would  connect  the  axis  controller  bus  to  the  host  bus,  and  would 
represent  a  relatively  small  fraction  of  the  total  system  complexity,  thus 
allowing  easy  migration  to  new  host  computing  platforms.  It  was  decided 
that  the  axis  controller  bus  should  be  the  IBM-PC  bus,  due  to  the  variety 
of  compatible  products  in  the  marketplace. 

3.  Robot  control  software.  Based  on  experience  with  RCCL  and  CSIRO’s 
ARCL  robot  controller(5]  it  has  been  decided  to  redesign  the  robot  control 
software  so  as  to  be  very  modular,  as  opposed  to  the  “monolithic”  struc¬ 
ture  of  RCCL.  The  structure  looks  like  comprising  a  number  of  simple 
interfaces  and  functional  blocks,  implemented  as  libraries,  and  on  which 
the  applications  prograunmer  can  build.  The  detailed  work  would  be  tack¬ 
led  by  Gaylord  Holder  as  a  Master’s  project.  A  number  of  considerations 
in  the  design  are: 

•  at  the  lowest  level  it  must  be  able  to  interface  with  the  existing  RCI 
interface  to  Unimate  controllers,  as  well  as  the  new  MMCS  hardware. 

•  at  the  highest  level  it  must  provide  a  similar  level  of  functionality  to 
the  RCCL  programming  environment,  since  this  is  one  (despite  its 
limitations)  with  which  many  workers  are  familiar.  Within  this  new 
programming  environments  different  programming  tools  will  hope¬ 
fully  spring  up  and  eventually  replace  RCCL. 

To  restate  this,  a  new  motion  controller  should 

•  t>e  based  on  a  fast  single  thread  processor 

•  contain  a  host  independent  and  modular  motor  interface 

•  be  accessible  via  a  small  and  modular  software  library 

The  remainder  of  this  document  is  concerned  'vitli  tlie  first  two  points  only. 


1.3  System  overview 

This  section  provides  an  overview  of  the  hardware  and  software  components  of 

MMCS. 

1.3.1  Control  Processor  and  Software 

The  control  processor  has  two  main  computations  to  perform 

•  High  rate  servo  control  loops  for  the  motors 
a  Slower  rate  trajectory  generation 

In  the  existing  Unimate  controller  the  servo  control  loop  functions  are  per¬ 
formed  per  axis  by  an  8  bit  6503  microprocessor,  see  Figure  1.1.  To  achieve 
high  joint  stiffness  and  dynamic  performance  a  sample  rate  approaching  IkHz 
is  desired.  There  is  no  way  that  a  process  running  under  Unix  (on  a  1988  vin¬ 
tage  workstation)  can  achieve  this  order  of  response,  thus  the  alternatives  are 
to 

1.  use  a  separate  processor  to  perform  the  high  speed  servo  calculations.  The 
software  could  run  on  “bare-metal”  or  under  a  real-time  operating  system. 

2.  perform  the  servo  computation  at  interrupt  level  in  a  Unix  device  driver. 

The  first  approach  offers  the  most  flexibility,  and  there  are  a  number  of 
possibilities  for  a  separate  processor  including 

•  680x0  VME  CPU  cards  manufactured  from  many  sources.  These  pro¬ 
cessor  boards  can  plug  into  the  VME  backplane  and  communicate  with 
the  host  via  shared  memory.  Having  the  same  instruction  set  as  the  host 
eliminates  the  need  for  software  cross  development  tools.  Communications 
and  support  software  could  be  developed,  or  an  off-the-shelf  package  such 
as  VxWork  ccuH  be  utilized. 

•  Bell  Labs  J.rr'E(3]  processor,  which  can  be  plugged  into  a  SUN  work¬ 
station,  and  has  full  software  support  including  a  C  compiler,  and  host 
communication  facilities. 

The  second  approach  is  lower  in  cost  but  not  as  flexible.  With  an  attached 
processor  the  user  can  code  up  an  experimental  servo  algorithm,  download  then 
run  it.  However,  a  servo  loop  in  the  kernel  means  that  the  user  would  have  to 
rewrite  the  driver,  link  a  new  kernel  and  boot  it.  Debugging  tools  exist  for  the 
kernel  but  they  are  primitive.  More  seriously  kernel  code  cann.^t  use  floating 
point  arithmetic.  Such  an  approach,  under  the  Xenix  operating  system,  has 
been  described[4]. 

A  variation  on  this  theme  is  the  RCl  package  written  by  John  Lloyd[9] 
in  which  the  kernel  interrupt  handler  invokes  a  user  process  function  in  kernel 
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Figure  1.1:  Notional  controller  structure 
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mode.  This  provides  run  time  linkage  of  user  code  into  the  kernel,  but  debugging 
remains  difRcult  and  the  user  code  must  obey  some  strict  guidelines. 

The  approach  taken  in  this  project  is  to  build  hardware  consistent  with  both 
approaches,  but  the  first  implementation  will  be  use  servo  loops  embedded  in  a 
device  driver.  The  driver  implements  a  very  general  servo  loop  capable  of  being 
configured  for  position,  velocity  or  torque  mode  operation.  Any  application  that 
wishes  to  can  bypass  the  servo  loops  and  specify  motor  currents  directly  (torque 
mode).  In  this  case  the  control  algorithm  is  running  in  a  user  process  and  its 
scheduling  cannnot  be  guaranteed,  with  possibly  serious  consequences  for  stable 
and  smooth  control.  This  is  unavoidable  when  working  under  Unix. 

Comparison  of  the  two  approaches  in  Figure  1.1  shows  that  the  functionality 
of  the  six  6503  servo  cards  in  the  Unimate  controller  has  been  shifted  to  the 
host  computer.  Simulations  of  the  servo  software  for  6-axis  computation  time 
was  done  for  a  number  of  processors  and  the  results  are  summarized  below 


Processor 

Time  (ms) 

Sun  3/160 

Micro  VAX  3500 

0.25 

MicroVAX  II 

This  indicates  that  Unix  device  driver  based  servo  loops  can  provide  satisfac¬ 
tory  sample  rates  on  most  of  the  GRASP  laboratory  machines.  The  simulation 
does  not  take  into  account  effects  such  as  adaptor  hardware  access  time,  inter¬ 
rupt  latency,  or  service  overhead  time. 


1.3.2  Motor  interface 

The  motor  interface  was  designed  with  the  following  design  aims 

•  make  it  as  host  bus  independent  as  possible 

•  medee  it  as  modular  as  possible 

•  use  as  much  of  the  proven  iSBX  design[7]  as  possible 

•  control  up  to  16  axes 

To  achieve  this  the  hardware  has  been  partitioned  into  three  components 

1.  Motor  interface  hardware  that  provides  current  drive  signal  to  the  motor 
and  processes  signals  from  sensors  regarding  the  motor's  state. 

2.  Axis  controller  bus  into  which  motor  interface  cards  are  plugged. 

3.  Host  adaptor  to  connect  motor  interfaces  to  a  host  computer  that  will 
perform  the  servo  computations. 


The  motor  interface  is  the  electronic^  that  connects  the  motor  to  the  axis 
controller  bus.  It  provides  an  analog  drive  signal  to  the  motor,  and  measures 
shaft  angle  via  an  incrementrd  encoder,  as  well  eis  application  speciRc  qusmtities 
via  a  general  purpose  analog  input. 

Control  of  a  system  with  upto  16  axes  introduces  a  number  of  problems 
such  as  timing  skew  between  sampling  the  first  and  last  axis.  If  the  system  is 
connected  to  a  Unix  host  computer  we  cannot  rely  on  software,  even  at  driver 
level  to  initiate  sampling  since  interrupt  latencies  can  vary  by  upto  100  /isec. 
Thus  it  was  seen  to  be  essential  that  sampling  is  controlled  by  a  hardware  clock, 
and  the  host  is  notified  by  interrupt  so  that  it  can  read  and  processs  the  state 
information.  The  hardware  clock  signal,  SCLOCK,  is  common  to  all  motor 
interface  cards. 

Since  the  host  is  also  interrupted  by  the  SCLOCK,  by  the  time  the  inter¬ 
rupt  handler  routine  is  entered  ail  state  information  is  available  to  be  read. 
This  means  that  A/D  conversion  time  is  overlapped  with  the  interrupt  service 
overhead  for  maximum  efficiency. 

Safety  considerations  indicate  that  every  motor  interface  should  have  the 
ability  to  indicate  its  readiness  for  operation  or  an  error  condition.  This  signal, 
referred  to  as  PANIC,  is  also  common  to  all  interface  cards,  anyone  of  which 
can  assert  the  line  to  indicate  a  system  failure.  The  type  of  failure  can  be 
determined  by  host  software  polling  all  cards. 

The  axis  controller  bus  needs  to  be  an  ordinary  computer  bus  with  address, 
data  and  control  signals,  but  it  also  needs  to  have  the  SCLOCK  and  PANIC 
signals.  It  was  initially  decided  to  define  and  use  a  custom  bus  for  this  purpose, 
but  later  the  decision  to  use  the  IBM  PC  bus  was  made.  The  PC  bus  is  nearly 
ideal  in  that  it  is  simple  to  interface  to,  emd  a  wide  range  of  peripheral  cards 
is  available  for  it.  The  two  special  signals  could  have  been  implemented  by  a 
separate  ribbon  cable  linking  all  boards,  but  this  is  not  failsafe  in  that  it  is 
possible  for  cards  to  be  not  connected.  Instead  two  signals  from  the  IBM  PC 
bus  were  “redefined”  for  these  purposes  and  is  discussed  further  in  Chapter  4. 
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Chapter  2 

Application  model  of  the 
motor  controller 


A  general  model  for  servo  motor  control  is  proposed.  It  is  capable  of  performing 
moat  commonly  required  functions  such  as  closed-loop  position  or  velocity  con¬ 
trol.  If  this  functionality  is  not  required  the  application  can  also  directly  specify 
motor  current  demands.  The  motor  controller  referred  to  here  comprises 

•  The  motor  interface  hardware  described  in  detail  in  Chapter  5. 

•  Servo  software  in  the  kernel  of  the  host  computer 

Since  future  interface  boards  may  incorporate  more  control  functionality,  an 
abstract  user  model  allows  the  software/hardware  balance  to  be  changed  without 
applications  being  recoded. 

Figure  2.1  shows  the  proposed  control  model.  A  number  of  switches  SI. .  .S5 
control  various  operating  modes  of  the  controller.  Feedback  can  come  from  one 
of  two  sources  (S4) 

•  An  incremental  shaft  angle  encoder 

•  An  A/D  converter 

A  derivative  block  may  be  switched  into  the  feedback  path  (S5)  which  will  cause 
a  velocity  servo  function  to  be  implemented.  The  setpoint  signal  may  come  from 
either  (SI) 

•  The  application  program  via  a  writef)  system  call,  which  is  represented 
by  8  in  Figure  2.1. 

•  The  A/D  convertor 

S3  switches  in  an  optional  Coulomb  friction  compensation  block,  while  S2  by¬ 
passes  the  feedback  control  and  allows  the  setpoint  to  control  motor  current 
directly. 
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Figure  2.1:  Motor  controller  block  diagram 


2.1  Compensator 

A  compensator  is  included  in  the  forward  path  to  allow  users  to  tailor  the 
dynamic  response  of  the  closed-loop  system.  Typically  for  a  DC  electric  servo 
motor,  the  transfer  function  is 


e  k 

where  6  is  motor  shaft  angle,  i  is  motor  current,  k  is  the  motor  torque  constant, 
J  is  the  motor  inertia  comprising  seif  and  reflected  load  inertia,  and  B  is  viscous 
friction. 

To  provide  position  control,  feedback  is  required,  and  to  achieve  good  dy¬ 
namic  peformance  and  disturbance  rejection  some  compensation  is  required. 


2.1.1  The  general  transfer  function 


The  controller  implements  a  unity  gain  negative  feedback  loop  on  position,  with 
a  general  discrete  transfer  function  compensator  as  shown  in  Figure  1.  The 
transfer  function  is 


Di!) 


-f  0|  -f  oq 
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where  the  coefficients  Of  and  6<  are  programmable  by  the  user.  The  DC  gain  is 
given  by  coefficients  and  quantities  are  32  bit  signed  integers, 

so  care  must  be  given  to  the  scaling  of  the  integer  coefficients. 

Many  design  methodologies  may  be  used  to  synthesize  the  compensator 
coefficients [6].  If  a  continuous  time  transfer  function  is  synthesized,  perhaps 
using  any  of  the  standard  forms  discusssed  below,  techniques  such  as  bilininear 
transform,  Z-transform  or  pole/zero  mapping[6]  may  be  used  to  generate  equiv¬ 
alent  discrete  time  transfer  functions.  Details  of  some  commonly  used  control 
strategies  are  given  below.  An  example  of  synthesis  for  a  PID  control  law  is 
given  in  Section  2.5. 


2.1.2  PID  implementation 

The  classical  continuous  time  PID  controller  has  a  transfer  function  of 


u  =  Pe  +  ^  J  edt 

where  e  is  the  error,  demanded  minus  measured  plant  output. 
Laplace  transformed  to 

t*  9 


This  may  be 


£ 

E 


Pa  +  Da^  +  l 


from  which  it  b  clear  that  the  transfer  function  has  a  pole  at  the  origin,  s  =  0, 
and  a  complex  pair  of  zeros  affected  by  the  parameters  P,  /,  and  D. 


2.1.3  PD  implementation 

The  transfer  function  of  a  PD  controller  is 

U 

^  =  P^Da 

which  has  a  zero  at  s  =  —P/D. 


2.1.4  PI  implementation 

The  transfer  function  of  a  PI  controller  is 


which  has  a  zero  at  s  =  —I/P.  and  a  pole  at  the  origin. 


2.2  Control  options 

2.2.1  Velocity  servo 

A  velocity  servo  loop  may  be  implemented  by  switching  in  a  differentiator  (S5) 
to  the  position  feedback  path.  The  differentiator  is  implemented  by  a  three 
point  derivative 

dy  ^  3yt  -  4yt-i  +  yt-i 
it  2 

to  yield  a  smoother  velocity  estimate.  Note  that  the  velocity  units  (see  S4)  are 
either  encoder  counts,  or  transformed  A/D  units,  per  sample  interval. 

2.2.2  Torque  servo 

For  torque  control,  the  compenstation  computation  is  completely  switched  out 
(S2),  and  the  user  specified  value  is  used  directly  as  motor  current  demand. 


2.2.3  Coulomb  friction  compensation 

Coulomb  friction  is  a  non-linear  effect,  in  which  an  approximately  constant 
torque  opposes  the  motor’s  torque.  The  friction  torque  is  not  necessarily  the 
same  for  each  direction  of  rotation,  and  varies  with  joint  loading,  and  will  thus  be 
somewhat  configuration  dependant.  A  v'ptional  Coulomb  friction  feedforward 
function  may  be  enabled  (S3)  to  compensate  for  this  non-linear  effect.  The 
compensator  implements  the  control  law 


•  ■+  icp 

•  ~  fen 


if^>0 

ife<0 


where  i  is  the  outnu  of  switch  S2. 

If  the  velocity  is  and  a  non-zero  current  is  specified  the  sign  of  the 
current  demand  (from  the  digital  compensator)  is  used,  since  that  indicates  the 
direction  of  desired  motion. 

iff  and  ten  are  the  currents  required  to  overcome  the  Coulomb  friction 
torques  in  the  positive  and  negative  rotational  directions  respectively. 


2.2.4  Feedback  source 

The  feedback  signal  (S4)  may  come  from  either  the  shaft  angle  incremental 
encoder  ff,  or  from  the  A/D  converter  associated  with  the  axis.  The  raw  data 
from  the  A/D  converter  is  processed  with  a  simple  linear  law 

0  =  aADC  +  6 

that  provides  scaling  and  offset  before  it  is  u.sed  as  the  feedback  signal. 


II 


For  example,  opening  the  feedback  path  (S5),  and  selecting  demand  from 
the  A/D  (SI),  the  servo  will  implement  a  programmable  digital  filter  between 
A/D  and  D/A.  Application  soflwjure  could  also  log  the  raw  or  filtered  signal. 

2.2.5  Setpoint  source 

The  setpoint,  or  demand  signal  may  come  from  one  of  two  sources  (SI)  as  shown 
in  Figure  2.1.  NormsJly  it  would  be  supplied  by  the  user’s  application  progrsun 
to  the  device  driver  via  a  write()  system  call.  However  it  may  be  selected  to 
xome  from  the  processed  A/D  signal, 


2.3  The  Unix  device  driver 

A  SunOS  device  driver  (/dev/mc)  has  been  written  to  implement  the  applica¬ 
tion  model  of  the  controller,  and  is  described  in  this  section.  The  me  device 
driver  does  not  support  the  many  individual  features  of  specific  motor  interfaces. 
To  access  these  capabilities  it  is  probably  more  effective  to  map  to  the  device 
hardware  from  Unix,  and  directly  manipulate  control  registers  as  described  in 
section  2.4. 

2.3.1  Configuring  the  servo 

Every  axis  has  a  paraimeter  structure  which  describes  the  mode  of  operation  to 
the  me  device  driver. 


ilnelnde  <aya/ncdef . h> 

/* 

•  Per  Joint  paraneter  structure 

*/ 


■c_paran 

int 

which; 

Int 

a3,  al,  aO,  b2,  bl, 

bO;  /«  coapenaator  coefficients  */ 

int 

ic.poe,  ic.neg; 

/*  feedforward  constants  */ 

int 

node,  clkdivisor; 

int 

ilin,  iliamu; 

/•  current  limit  */ 

int 

plo,  phi; 

/*  position  limits  */ 

int 

adc.a,  adc.b; 

/*  adc  conversion  Ian  */ 

int 

ipole; 

/•  current  filter  pole  */ 

The  units  for  ic-poa,  tc.  nrg  and  dim  are  D/A  convertor  units,  plo  and  phi 
are  in  the  units  of  whatever  feedback  source  is  selected,  clkdtvisor  is  the  number 
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of  hardware  clock  ticks  between  servo  computations  for  this  axis.  That  is,  each 
axis  may  be  servoed  at  a  sub-multiple  of  the  hardware  clock  rate. 

Possible  values  for  mode  are 


MD.OFF 

MDJ»OS 

MD.VEL 

MD.TORQ 

MD.TESTl 


Not  servoed 
Position  control  mode 
Velocity  control  mode 
Torque  (current)  control  mode 
Generate  triangle  waveform 


Additional  values  may  be  or’d  with  the  mode  word,  such  as 


COULCOMP 

ADFB 

ZEROFB 

ADDMD 

ADOFF 

POSCHK 

SOFTERR 


Enable  the  Coulomb  friction  feedforward  (S3) 
Feedback  comes  from  d  not  9  (S4) 

Zero  feedback,  that  is,  open-loop  operation  (S5) 
Demand  comes  from  d  not  the  computer  (SI) 
Torque  offset  comes  from  ^  not  the  computer 
Check  position  limits  on  feedback  signal 
Don’t  shut  motors  down  when  error  is  detected 


Note  that  not  all  switch  combinations  are  useful,  and  this  is  not  checked. 
The  servo  parameters  for  an  axis  are  set  or  examined  using  an  ioctl()  system 
call  on  the  me  device.  To  retrieve  parameters  from  an  axis  the  which  element 
must  first  be  set  to  indicate  which  axis  the  parameters  are  required  for 

struct  Bc.paraa  par; 

par. which  >  axis; 

ioctKmcfd,  HGETPARiM.  ftpar); 

mefd  is  the  file  descriptor  for  the  motor  control  device,  /dev/mcO.  To  set 
parameters  the  parameter  structure  should  be  initialized  by  the  user’s  program, 
and  the  which  element  set  to  indicate  which  axis  the  parameters  are  destined 
for. 


struct  ■c.pazaa  par; 

par. which  >  3; 
par.Bods  a  HD.VEL; 
ioctKwctd,  NSETPARAN.  kpar); 

Only  one  axis  may  be  initialized  per  iocll().  All  parameters  arc  initialized 
as  shown  in  Table  2,1  when  the  device  is  opened. 

Also  at  open  time  the  driver  scans  the  axis  controller  bus  looking  for  motor 
interface  cards  from  axis  0  through  axis  1.5.  When  the  device  is  closed,  all  motor 
currents  are  set  to  zero  and  the  PANIC  signal  asserted. 
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Structure  element 

Initial  value 

aO 

1 

al 

0 

a2 

0 

bO 

1 

bl 

0 

b2 

0 

ic-pofl 

0 

ic-neg 

0 

Him 

1/2  maximum  current 

ilimmax 

0 

adc-a 

1 

adc.b 

0 

mode 

MD.OFF 

clkdivisor 

1 

ipole 

0 

Table  2.1:  Initial  parameter  values 


2.3.2  Choice  of  parameters 

It  may  appear  that  there  is  an  overwhelming  number  of  parameters  to  set  before 
anything  can  be  done.  This  is  true,  and  unavoidable  with  the  general  appoach 
taken.  For  the  Unimate  controller  many  of  these  issues  are  handled  by  the 
6503  microprocessors  and  the  code  they  execute  from  EPROM.  The  Unimate 
servos  have  been  tuned  for  good  performance  with  the  motors  and  mechanicrd 
systems  used.  For  MMCS  these  parameters  must  be  determined  by  the  user, 
there  is  no  alternative.  The  interactive  tool  mctool  can  let  a  user  adjust  con¬ 
troller  paraiTwters  to  obtain  good  performance.  Another  approach  would  be  an 
adaptive  control  algorithm  that  modelled  the  motor  from  input/output  data 
and  computes  the  compensator  parameters  for  user  specifed  closed-loop  poles. 

The  controller  parameters  are  very  dependent  upon  the  type  of  motor,  the 
power  amplifier,  and  mechanical  drive  train.  Parameters  that  work  well  for  one 
motor  may  cause  instability  with  another. 

However  some  practical  hints  are  in  order 

•  Determine  which  direction  the  encoders  change  when  a  positive  torque  is 
applied  to  the  motor.  If  the  encoders  increase  in  a  negative  direction  then 
the  DC  gain  of  the  compensator  must  be  negative,  and  both  Coulomb 
friction  compensation  paramters  must  be  negative. 

•  For  position  mode  control  PD  control  is  appropriate,  for  velocity  mode  PI 
control  is  appropriate. 

•  The  mciximum  current  should  be  left  at  some  low  value  (default  is  half 
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maximum)  until  th«  controller  parameters  and  application  are  well  be¬ 
haved. 

2.3.3  Accessing  servo  state 

The  driver  maintains  state  information  for  each  axis. 


•  Per  joint  state  and  status 

•/ 

struct  Bc.state  f 


Int  erreode; 
>: 


latest  A/D  value  (processed)  e/ 
latest  encoder  value  e/ 
last  currant  command  issued  e/ 
latest  error  value  •/ 
latest  value  oi  Aaeback  quantity  */ 
latest  velocity  estimate  d/dt  {ibnow}  */ 
/*  current  errors  on  this  axis  */ 


iat 

adeval; 

/* 

Int 

snenov; 

/• 

iat 

inov; 

/* 

int 

error; 

/• 

Int 

fbnov; 

/• 

iat 

val; 

/* 

A  read{)  system  call  on  the  me  device  returns  a  vector  of  mediate  structures, 
•  which  may  be  used  by  the  user  as  required.  The  state  ’/ariables  are 

adcvttl  The  instantaneous  value  of  the  A/D  after  processing  via  the  linear  law. 
enenoro  The  instantaneous  value  of  the  incremental  encoder  counter. 

inow  The  instantaneous  or  filtered  motor  current  demand,  b  related  to  the 
torque  needed  to  maintain  the  position  or  velocity  demand  set.  It  will  be 
related  to  dbturbance  forces  such  as  gravity  or  robot/object  interactions. 
If  the  parameter  ipo/e  is  zero  inotcb  the  instantaneous  current.  If  non-zero, 
then  the  motor  current  demand  is  filtered  by  a  unity  gain  first  order  digital 
filter  whose  pole  b  at  ipole/MC~FSCALE,  and  the  value  of  inow  should 
be  divided  by  MC-FS(I!ALE  to  convert  from  fixed  point  filter  arithmetic 
to  real  value. 

error  The  instantaneous  error  between  the  feeback  quantity  and  the  demand. 

Jhnow  The  instantaneous  value  of  the  feedback  quantity,  which  will  always  be 
the  same  as  either  enenow  or  adeval . 

vtl  The  current  plant  output  velocity  estimate,  in  units  per  sample  period. 
erreode  The  last  error  code  that  occurred  for  this  a.xis. 
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/ 


MERR-PLO 

MERRJPHI 

MERRJLIM 

MERR_PANIC 

MERR-ZINDEX 

MERR-MASK 


Low  position  limit  crossed 
High  position  limit  crossed 
Sustained  current  overload 
Hardware  panic  detected 
Zero  index  detected  in  CALIB  mode 
Mask  for  actual  error  code  bits 


l^ble  2.2:  Device  error  codes 


State  information  is  always  available  once  the  me  device  is  open.  It  is 
updated  at  every  SCLOCK,  the  sample  interval  is  set  by  the  MSETINTERVAL 
iocti()  call. 

2.3.4  Error  handling 

The  MMCS  subsystem  can  generate  a  number  of  error  conditions.  The  error  sta¬ 
tus  for  each  axis  is  held  in  the  erreede  element  of  the  motor’s  state  structure.  The 
value  may  be  any  of  the  codes  shown  in  Table  2.2.  The  bit  MERR_POSERR, 
if  set,  indicates  that  a  position  error,  either  high  or  low  occurred.  Since  the 
state  structures  are  read-only  the  error  status  of  all  axes  can  only  be  cleared  by 
the  MCLEARERROR  ioctl()  which  will  usually  be  called  from  the  user’s  error 
handling  code. 

On  all  error  conditions,  and  zero  index  detection,  the  application  process 
is  notifed  by  a  signal  SIGUSRl.  If  the  operting  mode  of  the  axis  is  or’d  with 
SOFTERR  then  no  further  action  is  taken  by  MMCS,  and  the  user’s  signal 
handler  is  responsible  for  dealing  the  condition.  If  SOFTERR  is  not  set  the 
robot  shutdown  by  giving  a  zero  current  demand  to  all  motors,  and  activating 
the  brakes. 

The  driver  always  checks  for  sustained  torque  overdrive  of  the  motor.  If  the 
current  demand  exceeds  the  parameter  Him  for  more  than  ilimmax  samples  the 
MMCS  is  shutdown.  Motor  current  is  always  clipped  to  the  maximum  value 
allowed  by  the  D/A  converter.  If  ilimmax  is  zero,  then  motor  currents  are 
clipped  to  Him  and  no  error  condition  is  generated. 

If  mode  has  the  POSCHK  bit  set  then  the  feedback  quantity,  &om  encoder 
or  A/D  is  checked  against  the  limit  parameters  phi  and  plo.  The  error  condition 
is  generated  only  when  the  limits  are  crossed,  not  continuously  while  the  limit 
exists. 

A  hardware  panic  is  initiated  by  one  of  the  motor  interface  cards,  or  the 
hand  held  panic  button.  The  failsafe  nature  of  the  system  design  means  that 
panic  will  also  be  asserted  if  connections  such  as  that  between  host  and  MMCS, 
or  MMCS  and  panic  button  are  broken.  The  axis  number  bitfield  in  the  error 
code  is  meaningless  for  this  condition. 

When  an  axis  is  placed  in  calibrate  mode  using  the  MCALIB  ioctl(),  then 
the  first  detected  zero  index  will  send  a  SIGUSRl,  and  switch  that  axis  out  of 
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calibrate  mode.  The  high  order  16  bits  are  the  encoder  value  at  the  time  of  the 
tero  index. 


2.3.5  Other  device  driver  functions 

Functions,  not  already  discussed,  that  can  be  controlled  via  ioctl()  calls  are 
given  in  Table  2.3. 

2.3.6  Code  example 

Figure  2.4  is  a  code  fragment  that  illustrates  the  important  steps  in  controlling 
motors  via  the  me  device  driver. 


2.4  Accessing  hardware  directly 

This  approach  to  interfacing,  mapping  device  hardware  registers  to  a  Unix  pro¬ 
cess,  is  very  specific  to  the  flavor  of  Unix  being  used.  Some  likely  approaches 
are  Ultrix  via  the  /dev/bus  device  driver,  or  SunOS  via  the  /dev/vme*  device 
drivers.  User’s  following  this  path  should  be  very  familiar  with  the  material  in 
Chapters  4,  5  and  the  appendices.  A  SunOS  code  fragment  is  given  in  Figure 
2.5. 

For  more  details  consult  the  Unix  manual  entries  for  val1oc(2)  and  mmap(2). 
If  an  access  is  made  to  a  address  at  which  no  device  resides,  the  VMEbus  times 
out  and  a  SIGSEGV  (segmentation  violation)  signal  is  delivered  to  the  user 
process.  A  SIGBUS  (bus  error)  signal  can  be  delivered  if  the  device  hardware 
messes  up  the  VME  cycle. 

Note  that  accesses  to  devices  via  mapped  memory  cause  “non-priviliged" 
address  nwdifiers  to  be  issued  while  accesses  from  a  device  driver  cause  “priv- 
iliged”  address  modifiers[l]. 

2.5  Control  synthesis 

There  are  a  number  of  methods  of  transforming  a  continuous  time  system  to  a 
discrete  time  system,  such  as 

•  Pole/zero  mapping 

•  Bilinear  transformation 

•  Bilinear  transformation  with  frequency  prewarping 

•  Z-transform 

•  Z-transform  with  zero-order  hold 
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1  Request 

Comments  1 

MGETNUMCARDS 

int 

Return  the  number  of  motor  interface  cards 
present  in  the  axis  controller  bus. 

MSETNUMJOINTS 

int 

Specify  the  number  of  axes  that  will  be  co"*:./lled 
by  the  servo  code.  Returns  EINVAL  if  this  number 
is  greater  than  that  supplied  by  the  motor  inter¬ 
face  cards  present. 

MGETPARAM 

struct  tnc-param 

Return  the  parameter  utructure  for  the  axis  speci¬ 
fied  by  the  which  element  of  the  passed  parameter 
(read/write  argument).  Will  return  EINVAL  if 
which  is  greater  than  the  number  of  axes,  or  if  to 
is  equal  to  zero. 

MSETPARAM 

struct  mc-param 

Set  the  parameter  structure  for  the  axis  specified 
by  the  which  element  of  the  structure.  WiU  return 
EINVAL  if  which  is  greater  than  the  number  of 
axes. 

MSTOP 

Stop  all  axes,  set  all  motor  torques  to  zero,  joint 
control  modes  to  MD.OFF,  remove  enable  status, 
and  activate  brakes. 

MENABLE 

Check  that  all  boards  are  operating,  and  allow  all 

D/A’s  to  be  written. 

MSETVERBOSITY 

int 

If  M-VERBOSE  bit  is  set  then  the  driver  prints 
additional  diagnostic  information  during  opera¬ 
tion.  If  M_ERRORPRINT  bit  is  set  then  infor¬ 
mation  is  only  printed  during  error  situations. 

MGETVERBOSITY 

int 

Returns  the  verbosity  flag. 

MCLEARERROR 

Clears  the  error  status  for  all  axes. 

MSETINTERVAL 

unsigned  int 

Set  the  hardware  timer  interval  in  fiaee.  Return 
EINVAL  if  timer  is  incapable  of  meeting  the  inter¬ 
val  requested.  If  the  time  interval  is  greater  than 
the  heartbeat  timeconstant  in  the  motor  interface 
board  PANIC  will  be  asserted  by  hardwaire,  see 
5.3.6 

MGETINTERVAL 

unsigned  int 

Get  the  hardware  timer  interval  in  fiscc 

MSETEiNC 

unsigned  int 

Set  the  hardware  encoder  register  to  given  value. 
Lower  8  bit  specify  axis,  next  16  bits  specify  value. 

MSETIED 

int 

Control  axis  indicator  LEDs,  each  bit  in  the  argu¬ 
ment  controls  one  LED  associated  with  the  axis. 

Axis  is  specified  by  lower  8  bits,  LEDs  by  bits  8. . . 


Request 

Argument 

Ojmments 

MDIAGMODE 

int 

Specify  the  diagnostic  operating  mode.  If  bit 
M^NDIAG  is  set  then  analog  loopback  is  en¬ 
abled,  if  bit  M-ENL»1AG  is  set  then  encoder  loop- 
back  is  enabled.  EINVAL  is  returned  if  the  board 
cannot  perform  the  specified  diagnostic. 

MSETDAC 

int 

The  lower  8  bits  of  the  argument  specify  which 
axis  D/A  is  set  to  the  value  specified  by  the  next 
higher  12  bits. 

MDACMODE 

int 

If  argument  is  non-zero  then  DAC  double  buffer 
mode  is  enabled  for  all  axes,  otherwise  single 
buffered  mode  is  enabled. 

MGETSTATS 

struct  mc_stats 

Returns  a  structure  of  statistics  gathered  by 
the  driver  about  interrupts,  such  as  total,  those 
missed,  overrun  etc. 

MZEROSTATS 

Zero  the  driver’s  statistics  structure. 

MCALIB 

int 

Puts  the  axis  specified  by  the  argument  into  cal¬ 
ibration  mode.  When  a  zero  index  is  detected, 
the  user’s  process  is  notified  by  a  SIGUSRl,  and 
a  calibration  ‘error’  status  is  indicated. 

Table  2.3:  loctl  caUs  for  the  me  device 
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/♦ 

*  Exaapl*  prograa  showing  use  of  MMCS  system  for  2  axis  control  in 

*  Tslocity  mods  with  random  trajectories. 

* 

*  pic  1/89 

♦/ 

tinclnde  <stdio.h> 

tinclnde  <sys/f ile . h> 

•include  "/usr/sys/sundev/mcdef .h" 

•include  <signal.h> 

int  fd, 

naxis. 

Terbose, 
intval  =  2; 
int  setpCl6]: 

char  epnaae; 

•define  VAL(b)  (atoiUbCl])) 

mainCac,  aw) 
int  ac; 

char  *«aT; 

{ 

struct  mc.stats  stats; 

struct  mc.state  stats [16]; 

struct  mc.param  param; 
int  i; 

Toid  macserrO; 


pname  »  av  [0]  ; 

/• 

if  (ac  1)  ■( 

“sage:  fprintf (stderr ,  "Usage:  y.s  OVn”.  avCO]); 

sxit(l); 

} 

*/ 

while  ( — ac  >  0  »»  ••++av  ==  { 

register  char  ep  =  *avi 

while  {*+-t-p  !=  ‘\0’) 
switch  (♦p)  { 

case  *»':  Terbo8e++:  break; 

iftse  't':  intval  =  VAL(p);  break; 

) 


Figure  2.2:  MMCS  code  example 


■random(1234E6) ; 


if  ((id  »  opMiC/<l*v/»cO",  O.RDWR))  <  0)  { 
p«rror("op«n:**); 

•zlt(3): 

> 

•ignal(SIGt7SRl,  ■acsarr); 

if  (verbosa)  { 

i  «  H.VERBOSE; 

ioctKfd,  HSETVERBOSITY,  Ri); 

> 

if  (ioctKfd.  NGETIUMCAROS.  fti)  <  0) 
porror ( “ ioctl : " ) ; 

printfC'Xd  cards  in  systsaVn",  i); 
naxis  «  i*2; 

if  (ioctKfd,  HSETUmJOIITS,  Anaxis)  <  0) 
psrrorC'ioctl : ; 

intxal  *=  1000; 

if  (ioctKfd,  HSBTIITERVAL,  Aintval)  <  0) 
psrror("ioctl:") ; 


/* 

*  zero  the  sncoders 

*/ 

for  (i=0;  Knaxis;  i++)  { 
sstpCi]  »  0; 
ioctKfd,  HSETEJC,  Ai); 

/• 

*  initialize  the  paraneters 

*/ 

paran.ehlch  =  0; 
ioctKfd.  KGETPARAH,  Aparaa); 
paran.aO  =  SO; 
paran.bO  »  -1; 

paraa.moda  =  MD.VEL  I  SOFTERR  I  POSCHK; 
paran.plo  =  -6000; 
paran.phi  =  6000; 
paran. ilinnax  =  0; 

> 


Figure  2.3:  MMj^S  code  example 


lor  (1=0;  Knaxis;  i++)  { 
paru.shlch  =  i; 
ioctldd.  MSETPARiM,  tparaH); 

> 

11  (Ioctldd,  MEJABLE)  <  0)  { 

IprlntKatderr,  "cant  •nabloXn"): 
•zlt(3): 

.  } 

lor  (1*0;  Knaxis;  1++) 

astpCl]  =  vslocO; 

vrltsdd,  astp.  nazls  *  slzeol(lst)) ; 


/* 

*  The  program  nov  waits ,  all  control  la  done  In  a  signal 

*  handler  Invoked  when  an  axis  exceeds  its  position  limits. 

*/ 


> 


lor  (;;) 

sigpanseO; 


void 

mmcserrO 

{ 

struct  mc.state  state  [16]; 
Int  1,  code; 


readdd,  state,  slzeoKstate)) ; 
ioctldd,  KCLEAHERROR) ; 

lor  (1=0;  Knaxis;  i++)  { 

11  ((code  ■  stateCl]  .errcode)  *=  0} 
continue; 

11  ((code  k  MERR.POSLIM)  kk  (i>=3)  kk  (K=4))  { 
prlntlC'Xs  limit  on  axis  y,d\n’', 

(code  k  MERR.PLO)  ?  "low"  :  "high", 

1 

): 

/* 

*  choose  random  velocity  in  opposite  direction 

*  lor  error  axis 

♦/ 

setp[i]  »  -velocO  *  ab8(aetp[i])  /  setpCi] ; 

} 

else  22 

printlC'error  code  OxXx  on  axis  y.d\n",  code,  i); 

} 

writedd,  setp,  naxis  *  sizeoKint)} ; 

} 


velocO 

{ 


finclndfl 

tiaclod* 

iinclnda 

tlaclnda 


<aignal.h> 
<aja/lil«.b> 
<a7a/Kan.li> 
<a7a/t7paa .h> 


iat 


caddr.t 


laa. 

o«, 

buaarrO ; 
addr,  vallocO: 


/*  flla  daacrlptor  for  tha  bua  davica  */ 
/*  langth  of  naaory  vindov  to  map  a/ 

/a  baaa  of  aaaory  vindoa  a/ 


if  (laa  <  gatpagaaizaO)  /a  ronad  laa  ap  to  a  paga  aiza  a/ 
laa  >  gatpagaaizaO; 


fd  >  opaaC/daT/vaald**,  O.ROUR);  /a  opaa  tha  baa  davica  a/ 
if  (fd  <  0) 

parrorCopaa") ; 


addr  ■  valloc(laa);  /a  allocata  virtaal  aaaory  a/ 

if  (addr  »  lUU.) 

parrorCTalloc") ; 


/a 

a  aap  baa  aaaory  aiadow  iato  aaar’a  virtaal  aaaory 

a/ 

if  (aaap(addr,  laa,  PROT.REiDlPROT.WRITE,  HAP.SHARED,  fd,  off)  <  0) 
parror("Baap") ; 

aigaal(SIGBUS,  baaarr);  /a  aat  ap  aigaal  haadlara  a/ 

aigaal(SIGSEGV,  baaarr); 


baaarrO 

{ 

priatfC’BUS  BRRORNn"); 
loBgjap(aav,  1); 

> 


Figure  2.5:  SunOS  code  e.xampic  for  direct  aardware  access 
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Each  of  these  techniques  has  a  different  effect  on  characteristics  of  the  system 
such  as  DC  gain,  frequency  response,  overshoot  etc.  Standard  control  systems 
texts [8] [6]  can  provide  more  details. 

One  simple  approach  is  the  bilinear  transform 

2  z-1 
Tx+l 

where  T  is  the  sampling  interval.  The  Laplace  transform  expression  for  a  general 
PID  controller  is 

u  _  Ps  +  Da^  -h  I 
e  s 

where  P,  D  and  I  are  the  proportional,  derivative  and  integral  gains  respectively. 
Substitutitig  yields 

X-\ITI2  +  2D/T  -P)  +  x-\IT  -  4D/T)  +  (P  +  2D/T  -h  IT/2) 

_,-i  +  1 

which  is  in  the  form  implemented  by  the  controller’s  compensator.  The  coef¬ 
ficients  should  be  scaled,  so  that  all  are  greater  than  1,  since  all  arithmetic  is 
performed  in  32  bit  fixed  point. 
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Chapter  3 

mcTool 


meTool  is  an  interactive  graphical  tool  that  runs  under  the  SunView  window 
environment.  The  Sunview  Programmer’s  Guide  provides  some  details  on  the 
conventions  of  this  interface. 

When  invoked  mctool  opens  the  motor  control  device,  /dev/mcO,  and  deter¬ 
mines  the  number  of  motor  interface  cards.  A  panel  is  then  built  for  every  axis 
present.  The  topmost  panel  provides  global  controls  for  the  device,  in  particular 
■ample  interval.  The  enable  button  must  be  pressed  to  commence  servo  opera¬ 
tion  once  parameters  for  the  various  axes  have  been  set.  The  diagmodt  switch 
controls  analog  and  encoder  loop  back  modes  of  the  motor  interface  board,  see 
Chapter  5. 

mctool  reads  status  information  from  the  device  driver  five  times  per  second 
and  updates  the  displayed  values  for  encoder  count,  A/D  and  motor  current. 

Many  parameters  of  the  device  driver  may  be  altered,  by  typing  in  new 
numeric  values  or  adjusting  control  sliders.  Operating  modes  such  as  Coulomb 
friction  compensation,  LED  indicator,  feedback  source,  or  servo  mode  may  be 
controlled  by  switches. 

The  SeipSource  switch  allows  the  driver  setpoint  to  come  from  either 

a  the  Setpoint  slider 

a  the  A/D  converter 

a  a  square  wave  whose  amplitude  is  controlled  by  the  Amplitude  slider  which 
replaces  the  Setpoint  slider  in  this  mode. 

The  Mode  switch  controls  the  actual  servo  operating  mode.  Its  values  can 
be  any  of 

Off  this  axis  not  servoed  in  which  case  the  Setpoint  slider  b  not 

Pos  this  axis  b  in  position  servo  mode  and  the  labels  of  the  Setpoint  slider 
reflect  this,  displayed. 
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SatpSourca:  CSIIdar  Moda;  OOff  FtSaurea:  CEne 
LED:  OOff  CoutCcrap:  Ooff _ 


n:  [lee] 

0EIZ 

1  2388  A0C_a: 

Dl:  [0] 

0  1 - 

□  2M  Icr:  1 

Ic-: 

Tt:  t8] 

0  1 - 

□  200  Ilia:  1 

SatpSourca: 
LED:  COff 

n:  tiea] 

0  SHdar 
CoulCfrrp: 

Hoda;  COff 

Coff 

FSSourca:  CEnc 

0O_ 

1  2680  AOC_a: 

0»:  [0] 

8  8-  — 

31  263  Ic*:  1 

Ic-: 

»:  t0] 

0r~' 

3  208  Ilia:  1 

SatpSourca: 
LED:  COff 

Csildar 

CoulCosp: 

(toda:  COff 
COff 

FBSourca;  CEnc 

1  ADC 

Pt:  [tO0] 

0EIZ 

1  2088  *OC_a: 

W:  [0J 

0  1—  - 

3  289  Ic+:  1 

Ic-: 

H:  [0] 

0  1 

3  288  Ilia;  1 

SatpSourca: 
LED:  COft 

OSHdar  Noda:  0  Off 
CouICcap:  COff 

FBSourca:  CEne 

PS:  [180] 

0  E 

1  2080  *OC_a; 

OS:  [0] 

0  r-" 

]  200  Icr:  1 

Ic-: 

IS:  [0] 

8  i- 

3  208  Ilia:  t 

SatpSourca: 
LEO:  OOff 

OSlid6r  Modi:  OOff 
CouICcB^;  OOff 

FBSourca:  CEne 

^  ADC 

PS;  [100] 

— 1  2860  «C_a: 

OS:  [0] 

0  |- 

]  288  Icr:  1 

Ic-: 

IS:  [0] 

0r  ■ 

]  200  Ilia:  1 

SatpSourca: 
LEO:  OOff 

Csiidar 

CoulCoap; 

Moda:  OOff 
OOff 

FBSourca:  CEnc 

PS:  [100] 

0  88  " 

33)  2008  AOC_a: 

^  ADC 

Vel  this  axis  is  in^velocity  servo  mode  and  the  labels  of  the  Setpoint  slider 
reflect  this. 

Torq  this  axis  is  m  torq  servo  mode  and  the  labels  of  the  Setpoint  slider  reflect 
this. 

Testl  this  axis  is  in  MD.TESTl  mode  which  outputs  a  sawtooth  waveform  of 
amplitude  equal  to  the  present  current  limit,  Ilim. 

Diag  this  axis  is  not  servoed  by  any  adjustment  of  the  Setpoint  slider  causes 
that  value  to  be  output  to  the  D/A.  This  is  useful  for  testing  the  D/As  or 
setting  a  specific  output  voltage. 

''  The  PID  gain  sliders  have  units  of  %,  that  is  the  displayed  value  divided  by 
100  since  the  sliders  can  only  have  integer  values.  As  the  sliders  are  adjusted  the 
appropriate  compensator  coefficients  are  computed  and  modified  in  the  driver. 

Some  command  line  switches  for  mctool  are 

•tinterval  Set  the  initial  value  of  the  interval  time  to  the  specified  number  of  mil¬ 
liseconds. 

•u  Dont  update  the  encoder,  A/D  and  motor  current  state  information. 

•*  Zero  all  incremental  encoder  registers. 

Since  many  processes  can  open  the  me  device  at  the  same  time,  mctool  can 
be  used  as  a  monitor  of  the  motor  state  while  other  processes  are  controlling  it. 
In  this  situation  mctool  should  leave  all  axes  in  the 
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Chapter  4 

Host  adaptor 


4.1  Introduction 

A  fundamental  component  of  the  proposed  modular  motor  control  system  (MMCS) 
is  the  interface  between  the  host  bus  and  the  axis  controller  bus.  The  first  such 
interface  contructed  b  for  VME  bus  host  machines.  This  chapter  describes  the 
hardware  details  necessary  for  application  program  development. 


4.2  Adaptors  in  general 

Physically  an  adaptor  consists  of  two  cards  linked  by  a  ribbon  cable.  One  board 
plugs  into  the  host  bus,  and  the  other  plugs  into  the  axis  controller  bus. 

For  future  host  bus  adaptors  it  is  likely  that  the  existing  axis  controller  side 
of  the  adaptor  could  be  used,  necessitating  only  a  new  host  bus  card. 

4.2.1  Generic  specification  for  adaptors 

The  aulaptor  provides  five  main  functions 

1.  A  mapping  of  memory  accesses  on  the  host  bus  to  PC  bus  I/O  cycles  on 
the  axis  controller  bus. 

2.  A  programmable  source  of  clock  pulses,  SCLOCK,  used  to  synchronize 
the  latching  of  state  measurements  in  all  motor  interface  cards. 

3.  A  facility  to  interrupt  the  host  processor  on  two  conditions,  clock  pulse 
SCLOCK  and  control  bus  detected  panic  condition  P\N1C. 

4.  A  general  purpose  clock  signal  (around  8MIIz)  must  be  supplied  to  the 
PC  bus  for  use  by  motor  interface  boards. 
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5.  Safety  features  such  as  interfacing  to  the  handheld  panic  button,  and 
control  of  mechanism  brakes  if  present. 

4.2.2  PCbus  signals  redefined 

Two  PC  bus  signals  have  been  redefined  for  this  application.  From  a  purist’s 
standpoint  this  is  bad  design,  but  is  unavoidable  unless  a  separate  cable  was 
run  Unking  all  motor  interface  cards,  or  we  adopted  a  different  bus  altogether. 
Some  care  was  taken  in  choosing  which  Unes  to  use  for  these  special  purposes. 

1.  SCLOCK  signal,  used  to  control  latching  on  all  motor  interface  cards. 
This  signal  uses  one  of  the  PC  bus  DMA  request  lines  DRQl. . .  DRQ3.  It 
is  jumper  selectable  on  the  adaptor  and  motor  interface  cards,  and  should 
be  set  such  that  all  cards  use  the  same  Une,  and  that  the  Une  does  not 
conflict  with  other  I'C  bus  devices. 

2.  PANIC  signal,  u:  .:d  to  indicate  that  one  of  the  motor  interface  cards  has 
detected  an  error  condition.  This  line  is  a  wind  or,  that  is,  any  board  can 
puU  it  high  to  assert  the  PANIC  condition,  which  is  then  available  to  all 
other  boards. 

4.3  In  particular:  VMEbus  adaptor 

Physically  the  adaptor  is  two  cards  linked  by  an  8  foot,  SOway  ribbon  cable. 
One  board  plugs  into  the  VME  host’s  backplane,  and  the  other  plugs  into  the 
control  (PC)  bus.  The  VME  end  is  based  upon  a  Logical  Design  Group  VMEl- 
5100D  VMEbus  prototype  card  with  only  10  extra  chips.  The  PC  end  is  based 
on  a  bare  wirewrap  prototype  module. 

4.3.1  VME  memory  Map 

The  adaptor  occupies  2kbytes  of  VME  A 16  address  space,  layed  out  as  shown 
in  Figure  4.1.  It  comprises  two  regions,  one  that  is  mapped  to  PC  bus  requests, 
and  another  that  is  adaptor  control  regbters.  The  current  configurable  base 
address  of  this  segment  is  0x6000. 

The  adaptor  only  responds  to  8(0)  data,  that  is  byte  data  at  odd  addresses. 
Short  (16  bit)  requests  to  even  addresses  (odd  address-l)  will  transfer  the  bot¬ 
tom  byte  of  the  short  to  the  adaptor,  the  top  byte  is  ignored  on  write,  and  Oxff 
on  read. 

4.3.2  PC  bus  access 

When  A 10  is  0,  all  accesses  to  the  adaptor  are  passed  straight  through  to  the 
PC  slave  bus.  These  accesses  become  I/O  space  requests  on  the  PC  bus  and 
occupy  the  odd  addresses  0x001  through  0x3FF  on  the  VME  bus. 
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VME  address 


15 


adaptor  select 


PCbus  i/o  address 


Figure  4.2;  PC 


0  pcbus  leq 
1  adaptor  leq 


space  address  formation 


The  PC  bus  allows  for  16  bit  I/O  space  addresses.  IBM  however  define 
only  unique  values  or  ranges  for  the  least  significant  9  bits  of  the  I/O  address. 
The  address  range  for  prototype  card,  which  we  use  for  motor  interface  cards, 
ia  0x300  to  0x31f.  However  this  is  not  enough  address  bits  to  support  upto  16 
interface  cards,  each  with  uptu  32  bytes  of  registers.  We  thus  use  address  bits 
10. . .  13  to  peform  the  board  select  function,  and  bits  0  through  4  for  register 
•elect. 

The  address  mapping  between  VME  and  PC  I/O  space  is  shown  diagramat- 
icaly  m  Figure  4.2.  VME  address  bits  AOl. . .  AOS  are  mapped  to  PC  address 
bits  AOO. . .  A04.  VME  address  bits  A06. . .  A09  are  mapped  to  PC  address  bits 
AlO. ..A13.  PC  address  bits  AOS... AOO  are  set  via  the  control  register.  PC 
address  bits  A14  and  AIS  are  fixed  at  0. 

For  the  motor  interface  cards,  VME  address  bits  AOl. .  .AOS  will  specify  a 
device  register  byte  on  an  interface  card.  VME  address  bits  A06.  . .  A09  specify 
which  controller  card  is  being  selected  on  the  PC  bus.  Thus,  a  bank  of  up  to 
sixteen  controller  cards  is  mapped  as  a  contiguous  space  of  addresses  on  the 
VME  bus. 

4.3.3  Adaptor  Control  Registers 

When  VME  addresss  bit  AlO  is  1,  an  adaptor  register  is  selected.  These  registers 
include  the  interrupt  vector  register,  the  PC  control/status  register  (PCCSR), 
and  the  8254  Programmable  Interval  Timer  (PIT). 

When  address  bit  A06  is  a  1  on  the  VME  bus,  the  vector  register  is  selected. 
This  puts  the  vector  register  address  at  0x0441.  A  single  byte  may  be  written 
to  this  register  to  supply  a  vector  number  used  during  interrupt  service  on  the 
VME  bus.  This  register  may  not  be  read. 

When  address  bit  A06  is  0,  one  of  the  other  registers  is  selected.  When  VME 
addresses  AOS. . .  A03  are  (respectively  in  binary)  000,  the  PIT  is  selected.  In 
this  case,  A02  and  AOl  specify  the  register  on  the  PIT.  Thus,  the  PIT  regbters 
are;  timer  0,  0x401;  timer  1,  0x403;  timer  2,  0x405;  control  register,  0x407. 

When  A06  is  low  and  AOS. ..  A03  are  001  (binary),  the  PCCSR  is  selected. 
Thus,  its  address  is  0x409.  The  bitfields  of  this  register  are  shown  in  Table  4.3.3. 

4.3.4  The  Servo  Clock 

The  servo  clock,  SCLOCK,  ia  the  output  of  timer  1  on  the  8254.  It  is  clocked 
by  timer  0,  which  is  in  turn  clocked  by  the  8  MHz  system  clock  which  is  derived 
from  the  VME  bus.  The  system  clock  may  be  configured  down  to  4,  2,  or  1 
MIlz,  and  is  also  passed  to  the  PC  bus.  Timers  0.  ..2  are  permanently  gated 
on. 

Since  timers  1  and  2  are  cascaded,  allowing  a  count  of  32  bits  to  be  made  at 
8  .MHz,  a  very  wide  range  of  servo  loop  limes  is  achievable.  SCLOCK  clocks  an 
edge  triggered  latch  which  can  cause  a  VME  bus  interrupt  request.  The  servo 
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clock  is  also  passed  to  the  PC  bus  on  the  DRQ  3  line,  for  use  by  the  motor 
interface  cards. 

The  SCLOCK  signal  is  also  fed  to  timer  2  which  may  be  used  to  by  driver 
software  to  check  that  interrupts  are  not  being  missed,  that  is  at  each  interrupt 
timer  2  is  only  1  different  from  its  value  at  the  last  interrupt. 

4.3.5  Panic  signal 

The  panic  signal  from  the  PC  bus  is  logically  combined  with  the  status  of 
the  handheld  emergency  stop  button,  and  used  to  generate  the  system  PANIC 
signal.  The  rising  edge  of  this  signal  clocks  an  edge  triggered  latch  which  can 
cause  a  VME  bus  interrupt  request. 

4.3.6  Interrupts 

Interrupts  are  controlled  by  the  PC  control  register  (PCCSR).  When  bit  7  of  the 
PCCSR  is  low,  the  two  interrupt  latches  are  forf.;d  to  clear.  They  will  remain 
cleared  until  bit  7  is  set  high.  In  this  state,  a  low-to-high  transition  of  SCLOCK 
will  set  the  servo  clock  interrupt  latch,  and  a  low-to-high  transition  on  PANIC 
sets  the  panic  interrupt  latch.  They  will  remain  high  until  cleared  by  clearing 
bit  7.  Both  latches  are  cleared  together,  so  an  interrupt  service  routine  must 
poll  both  interrupts  and  service  the  appropriate  ones  before  clearing  bit  7. 

The  status  of  the  latches  is  read  back  from  bits  5  and  6.  To  enable  the  latches 
to  interrupt  the  VME  bus,  a  I  must  be  written  into  bit  5  or  6.  If  interrupts  are 
used,  don’t  forget  to  set  the  vector  latch,  see  section  4.3.3. 

Currently  the  VME-5100D  is  set  to  interrupt  at  VME  level  5,  which  is  at 
the  same  level  as  the  clock  and  Unix  scheduler  for  a  Sun  3[2].  To  alter  the 
level  both  jumpers  J5  and  J7  on  the  VME-5100D  must  be  altered.  Be  sure  to 
remove  the  interrupt  acknowledge  (lACK)  jumper  from  the  backplane  for  the 
slot  containing  the  VME-5100D.  A  “spurious  level  ?  interrupt”  message  on  the 
Sun  console  indicates  that  the  lACK  daisy  chain  is  incorrectly  jumpered  or  that 
J5  and  J7  on  the  VME-5100D  are  inconsistent. 

4.3.7  LED  indicators 

The  adaptor  card  has  two  LED  indicators.  The  green  LED  is  the  SCLOCK 
signal,  while  the  red  LED  is  the  status  of  the  user  interrupt  request  to  the 
VME-5100D  interrupter  logic. 

4.3.8  Miscellaneous  Notes 

VME  bus  resets  is  passed  to  the  PC  bus.  The  reset  signal  also  resets  the  PCCSR 
to  0x00  (interrupts  off). 
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The  PC  side  of  the  adaptor  may  be  powered  down  while  the  system  is  running 
without  affecting  the  VME  side.  To  disconnect  the  VME  side  from  a  SUN  3, 
the  SUN  must  be  shut  down,  powered  off,  and  the  VME-5100D  board  and  3U- 
2U  adapter  must  be  removed  from  the  SUN  backplane,  and  the  lACK  jumper 
installed. 


■:H1 

Write  operation 

Read  operation 

the  PC  bus  address  bits 
AOS  through  A09 

what  was  written  to 
those  bits 

5 

when  set,  enables  inter¬ 
rupts  from  SCLOCK 

Bit  5  is  1  if  an  SCLOCK 
interrupt 

has  been  latched  (but  not 
necessarily  passed  to  the 
VME  bus) 

6 

when  set,  enables  panic 
interrupts  from  the  PC 
bus 

Bit  6  is  a  1  if  a 
panic  interrupt  has  been 
latched  (also,  not  neces¬ 
sarily  passed  to  the  VME 
bus) 

1 

controls  the  clearing  of 
the  two  interrupt  latches. 
When  it  is  0,  the  latches 
get  cleared.  When  it  is  a 
1,  the  latches  may  get  set 

returns  what  was  written 
lo  it. 

Table  4.1;  PCCSR  bitfields 
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Chapter  5 


The  Mark  I  motor 
interface  card 


The  motor  interface  is  the  electronics  that  connects  the  motor  to  the  axis  con¬ 
troller  bus.  It  provides  an  analog  drive  signal  to  the  motor,  and  measures  shaft 
angle  via  an  incremental  encoders,  as  well  as  application  specific  quantities  via 
a  general  purpose  analog  input. 

This  chapter  provides  hardware  specifications  and  details  to  those  ni^eding 
to  program  the  device  hardware  directly. 


5.1  Servo  board  specification 

5.2  Design  aims 

One  of  the  most  important  design  features  is  to  allow  for  the  non-deterministic 
timing  of  the  host  computer,  targetted  in  the  first  instance  to  be  a  SUN  workstar 
tion  running  Unix.  The  use  of  am  additional  CPU  running  a  real-time  operating 
system  would  provide  better  performance,  albeit  at  greater  cost. 

At  the  short  sample  times  required  (less  than  10ms)  a  Unix  process  cannot 
be  reliably  scheduled  to  respond.  However  a  device  driver  working  at  high 
hardware  priority  will  respond  within  100/is,  but  will  not  be  deterministic  due 
to  interrupts  being  locked  out  in  critical  regions  of  the  Unix  kernel.  Thus,  it 
was  not  desirable  for  the  data  sampling  to  be  controlled  directly  by  the  host 
computer. 

Instead,  sampling  is  controlled  by  an  axis  controller  bus  signal  SCLOCK, 
and  at  each  sample  all  A/D  converters  and  encoder  registers  are  latched,  and 
the  host  notified  by  interrupt  that  new  data  has  arrived.  The  host  device  driver 
can  then  read  the  robot  state  and  compute  new  setpoints  to  be  written  to  the 
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D/A  converters.  State  is  thus  sampled  at  a  fixed  intervals  with  zero  timing 
error.  Additionally  the  D/A  converter  updates  can  occur  in  a  double  buffered 
mode  in  which  the  new  values  are  not  output  until  the  next  sample  time. 


5.3  Description 

Each  board  provides  all  the  functionality  required  to  control  two  servo  axes, 
referred  to  here  as  joint  1  and  joint  2.  Each  controller  consists  of  an  A/D,  a 
D/A,  and  an  incremental  encoder  interface  (lEI). 

•  AD574  12  bit  analog  to  digital  converter.  This  may  be  used  to  input 
application  specific  signals  such  as  joint  angle,  torque,  current  etc. 

•  AD667  12  bit  digital  to  analog  converter  for  specifying  motor  current  or 
torque.  The  device  has  the  ability  to  double  buffer  the  digital  commands. 

•  HCT2000  incremental  encoder  interface  (lEI)  to  decode  quadrature  signals 
from  incremental  encoders.  This  device  has  16  bit  resolution,  and  can  be 
read  or  written  at  anytime.  It  also  has  the  facility  for  the  count  to  be 
latched  by  an  external  signal,  which  we  use  for  calibration  purposes. 

•  i8255  to  provide  control  of  all  board  operating  modes  and  to  provide  status 
information.  The  8255  device  provides  24  programmable  input  or  output 
lines.  The  usage  in  thb  interface  is  summarized  in  Table  5.1. 

The  D/As  contain  two  cascaded  12  bit  latches.  The  D/A  must  be  accessed 
twice  to  load  a  new  value  into  the  first  latch,  once  for  the  low  byte,  and  once 
for  the  high  nibble.  D/A  data  is  12  bit  offset  binary.  The  loading  of  the  second 
latch,  and  hence  the  D/A  output,  is  controlled  by  port  B  bit  3  of  the  8255  device 
to  occur  either  when  the  high  nibble  is  written,  or  on  the  edge  of  the  SCLOCK 
signal. 

The  A/Ds  convert  all  12  bits  on  the  edge  of  the  SCLOCK.  The  12  bit  offset 
binary  values  must  be  read  in  two  byte  accesses.  The  status  of  the  conversion  can 
be  read  via  port  C  bit  4  of  the  8255,  which  when  set,  indicates  both  converters 
are  done. 

Data  sheets  on  these  devices  are  provided  in  an  Appendix. 

5.3.1  Memory  map 

The  memory  map  of  the  motor  interface  card  given  in  Figure  5.1  shows  the 
addresses  as  seen  from  the  PC  bus.  All  of  the  addresses  given  are  relative  to 
the  board’s  base  address,  and  are  five  bits  only. 

The  board  resides  at  address  0x300  to  0x31F  in  9  bit  (IBM  standard)  PC 
I/O  space.  However  this  is  not  enough  address  bits  to  support  upto  16  interface 
cards,  each  with  upto  32  bytes  of  registers.  We  thus  use  address  bits  10. . .  13  to 
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:  Servo  board  memory  map 


38 


peform  the  board  select  function,  and  bits  0  through  3  for  register  select.  Thus, 
0x0300  is  the  base  of  board  0,  and  0x3F00  is  the  base  of  board  fifteen.  The 
board  address  is  set  by  a  4  bit  DIPswitch  on  the  card,  when  a  switch  is  on,  or 
closed,  it  corresponds  to  a  0  bit. 

The  8255  programmable  peripheral  interface  (PPI)  is  at  addresses  0x00 
through  0x03. 

The  HCT2000  for  joint  1  is  at  addresses  0x04  through  0x05,  while  that  for 
joint  2  is  at  addresses  0x06  through  address  0x07.  The  most  significwt  byte 
occupies  the  lower  address.  The  HCT2000  may  be  written  to  initialise  the  16  bit 
up/down  counter.  It  may  be  read  at  anytime  to  give  the  instantaneous  counter 
value,  however  the  first  read  after  a  latch  command  will  yield  the  counter  value 
at  the  time  of  the  latch.  The  operating  mode  of  the  HCT20C0  is  set  via  port  A 
of  the  8255,  and  would  normally  be  mode  5,  although  a  number  of  other  modes 
such  as  frequency  and  interval  measurement  may  be  useful.  The  HCT2000’s 
two  byte  registers  should  always  be  accessed  in  the  same  order  after  reset,  see 
the  applications  note  for  more  details. 

The  D/As  are  at  addresses  0x08  through  OxOB.  These  locations  are  write- 
only.  Location  0x08  is  the  low  byte  of  D/A  1,  and  0x09  is  the  high  nibble  of 
D/A  1.  Similarly,  0x0 A  and  OxOB  write  to  D/A  2. 

The  A/Ds  are  at  addresses  OxOC  through  OxOF.  These  locations  arc  read¬ 
only.  Location  OxOC  returns  the  low  byte  of  A/D  1.  Location  OxOD  returns  the 
low  byte  of  A/D  2.  Location  OxOE  returns  the  high  nibbles  of  both  A/Ds.  The 
high  nibble  of  A/D  1  will  reside  on  bits  0  through  3  of  this  byte,  and  A/D  2 
will  use  bits  4  through  7. 

5.3.2  The  latch  signal 

The  latch  signal  is  used  to  start  the  conversion  of  the  A/Ds,  enable  the  output 
latch  on  the  D/As,  and  cause  the  lEs  to  latch  their  counts. 

This  latch  signal  may  come  from  either  the  rising  edge  of  SCLOCK  (a  back¬ 
plane  signal  from  the  host  adaptor)  or  be  generated  via  addressing  (see  Figure 
5.1).  To  enable  latch  on  the  rising  edge  of  SCLOCK  port  B  bit  2  of  the  8255 
must  be  asserted. 

Any  access  to  location  OxIO  will  cause  a  latch  signal  to  the  A/Ds  and  lEIs, 
just  as  the  SCLOCK  signal  does.  Any  access  to  0x12  will  do  the  same,  except 
that  it  will  cause  all  boards  in  the  controller  to  simultaneously  latch.  Jumper 
E021...E023  (see  Section  5.4.3)  control  whether  or  not  the  D/A  responds  to 
the  software  latch  signals. 

5.3.3  D/A  double  buffering 

The  D/A  converters  are  capable  of  working  in  normal  or  double  buffered  mode. 
In  norma!  mode,  the  D/A  analog  output  reflects  exactly  what  is  written  to  it. 
In  double  buffered  mode,  the  analog  output  changes  to  the  last  written  value 
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Comments 


Port  A:(OUTPUT 


Control  the  operating  mode  for  both  lEls  (HCT2000  chips). 
Diagnostic  LED  1. 

Diagnostic  LED  2. 

Artificial  encoder  A  signal  for  lEIs. 

Artificial  encoder  B  signal  for  lEIs. 

Artificial  encoder  I  signal  for  lEIs. 


Port  B:(OUTPUT 


When  0,  the  artificial  encoder  signab  are  passed  to  the  HCT2000s. 
When  1,  encoder  signals  from  the  joints  are  passed  through. 

When  0,  the  output  of  the  D/As  b  looped  back  to  the  A/Ds.  When 
1,  the  output  of  the  D/As  b  switched  to  the  joints. 

This  bit  must  be  a  1  for  the  external  servo  clock  on  the  PC  bus  to 
affect  this  board. 

This  bit  controls  the  output  mode  of  the  D/As.  When  0,  writing  to 
the  high  address  of  a  D/A  causes  its  output  to  be  updated.  When 
1,  the  internal  servo  clock,  conditioned  by  Bit  2,  causes  both  D/A 
outputs  to  be  updated. 

When  0,  the  index  latches  (see  Port  C,  bits  1  and  3)  are  cleared. 
When  1,  the  index  latches  get  set  on  the  next  index  pulse.  Also, 
when  thb  bit  b  a  1,  index  pubes  cause  the  lEIs  to  latch. 
Unassigned 


Port  C:(INPUT 


Thb  returns  the  status  of  the  heartbeat  for  joint  1.  A  1  indicates 
that  all  b  well.  The  heartbeat  gets  retriggered  when  the  low  byte 
of  the  D/A  gets  written  to. 

This  is  a  1  if  the  index  for  joint  1  has  been  latched.  The  index  gets 
latched  when  the  index  latch  enable  is  set  to  1  (Port  B,  bit  4)  and 
a  high  level  is  seen  on  the  index  input. 

The  same  as  Bit  0,  but  for  joint  2. 

The  same  as  Bit  1,  but  for  joint  2. 

This  signal  b  high  when  the  A/Ds  are  not  converting.  A  low  to 
high  transition  on  this  signal  means  that  good  data  is  in  the  data 
latches  for  the  A/Ds. 
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synchronously  with  the  latch  signal.  Thus  guarantees  a  fixed  one  sample  time 
delay  between  reading  state  and  providing  a  new  setpoint  value. 

5.3.4  Calibration 

Calibration  of  incremental  encoders  is  important  for  control  of  many  manipu¬ 
lators.  Frequently  the  procedure  involves  driving  the  axis  so  that  the  encoder’s 
sero  index  is  detected,  and  measurmg  the  shaft  angle  with  some  low  resolution 
absolute  transducer  (such  as  a  potentiometer),  and  then  computing  the  correct 
value  for  the  encoder  register.  In  RFMS  the  axes  were  driven,  the  zero  index 
status  polled,  and  the  motor  stopped  when  the  index  is  detected.  It  was  found 
necessary  to  drive  the  motors  very  slowly  else  the  index  would  be  missed. 

The  new  interface  provides  a  calibration  mode,  in  which  the  motor  can  be 
driven  at  any  speed,  and  when  the  index  pulse  is  detected,  the  instantaneous 
encoder  value  is  latched  and  the  host  notified.  To  enable  this  mode  port  B 
bit  4  of  the  8255  should  be  asaeried.  Port  C  bits  1  and  3  indicate  that  the 
lEls  for  joints  1  and  2  respectively  have  been  latched.  The  lEIs  can  then  be 
read  to  determine  the  encoder  count  at  which  the  index  pulse  occured,  another 
encoder  read  to  obtain  the  current  encoder  counts,  and  determination  of  motor 
potentiometer  voltages  provides  all  the  information  needed  to  determine  the 
absolute  motor  angle. 

Note  that  subsequent  index  pulses  are  not  locked  out,  and  will  also  latch  the 
encoder  count.  Detection  of  an  index  pulse  does  not  stop  the  motor,  this  must 
still  be  done  by  host  software  upon  detection  of  the  index  latched  status. 

5.3.5  Diagnostics 

The  board  has  a  number  of  diagnostic  test  facilities  built  in.  Firstly  the  input 
of  the  A/D  converters  can  be  “looped  back”  to  the  D/A  outputs,  which  allows 
testing  of  the  A/D,  D/A  devices  and  the  analog  signal  paths.  The  loopback  is 
done  via  a  relay,  so  some  short  time  should  be  allowed  for  the  relay  contacts  to 
close.  This  mode  is  enabled  by  setting  8255  port  B  bit  1. 

Secondly,  syn.hetic  encoder  signals  A,  B  and  I  can  be  fed  into  the  lEIs  to 
test  their  operation.  The  synthetic  signals  are  the  same  for  both  axes,  and  come 
from  8255  port  A  bits  5. . .  7.  This  mode  b  enabled  by  clearing  8255  port  B  bit 
0. 


5.3.0  Panic  signal 

One  signal  on  the  backplane  is  the  active  high  PANIC  line,  that  may  be  asserted 
by  any  board  that  detects  a  failure.  The  only  failure  detected  by  this  interface 
card  is  a  failure  to  regularly  update  the  D/A  converters  with  new  setpoint  values. 
The  D/A  write  signal  drives  a  oneshot  which  has  a  50ms  timeconstant.  The 
outputs  of  the  oneshot  (one  per  axis)  are  called  heartbeats  (since  they  indicates 
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Figure  5.2:  Motor  interface  board  layout 


something  is  going  on),  and  can  be  checked  by  onboard  LED  indicators  and  by 
software  via  port  C  bits  0  and  2  of  the  8255. 

Note  that  the  “peinic”  condition  will  not  go  away  until  all  the  D/A  converters 
have  been  written  to  initially.  Note  also  that  for  SCLOCK  intervals  longer 
than  the  heartbeat  timeconstant  PANIC  will  be  asserted  regularly,  it  weis  not 
considered  that  servoing  at  less  than  20Hz  would  be  useful. 


5.4  Board  details 

Details  of  the  motor  interface  board  layout  are  given  in  Figure  5.2. 

5.4.1  Switches 

There  are  two  switches  on  the  card,  one  per  a.xis.  Their  purpose  is  to  take  an 
axis  “out”  of  the  controller  without  having  to  remove  a  card,  or  half  a  card.  In 
the  enable  position  everything  works  as  described.  When  disabled,  the  analog 
current  drive  for  the  axis  is  switched  from  the  D/A  output  to  ground,  and  the 
heartbeat  signal  is  ignored,  thus  panic  won’t  be  caused  when  that  axis’s  D/A  is 
not  upd.ated. 


/ 


5.4.2  LED  indicators 


Comment 


Encoder  A  sign2d  for  joint  1. 

Encoder  B  signal  for  joint  1. 

Encoder  index  signal  for  joint  1. 

Encoder  A  signal  for  joint  1. 

Encoder  B  signal  for  joint  1. 

Encoder  index  signal  for  joint  1. 

Heartbeat  signal  for  joint  1. 

Heartbeat  signal  for  joint  2. 

General  purpose  (softwue  controllable)  indicator  for  joint  1. 
General  purpose  (software  controllable)  indicator  for  joint  2. 
Board  5V  supply  is  OK. 


Table  5.2:  Motor  interface  LED  indicators 


5.4.3  Configuration 

There  are  four  groups  of  jumpers,  as  described  in  by  Table  ?? 


From  To 


EOOl  E002 


Comment 


A/Dl  lOV  range 
A/Dl  20V  range 


A/D2  lOV  range 
A/D2  20V  range 


If  closed  D/Al  lOV  span 
If  closed  D/A2  lOV  span 


D/A  does  not  respond  to  software  latch 
D/A  responds  to  software  latch 


Sample  clock  is  DRQl 
Sample  clock  is  DRQ2 
Sample  clock  is  DRQ3 


Panic  IS  IRQ3 
Panic  is  IRQ4 
Panic  is  1RQ5 
Panic  is  IRQ6 
Panic  is  IRQ7 


Table  5.3:  Mark  1  configuration  jumpers 

They  are  used  to  set  voltage  scaling  for  the  A/D  and  D/A  converters  as  well 
as  to  select  which  PC  bus  lines  are  used  for  the  SCLOCK  and  PANIC  signals. 


; 


